﻿#include "urparser.h"
#include "ur_rtde/rtde_control_interface.h"
#include "ur_rtde/robot_state.h"
#include <QDebug>
#define _USE_MATH_DEFINES
#include <iostream>
#include "math.h"


URParser *URParser::_urparser=nullptr;


URParser::URParser()
{

}

URParser *URParser::Instance()
{
    if(URParser::_urparser==nullptr)
    {
        _urparser=new URParser();
    }
    return _urparser;
}

void URParser::connect(QString ip,unsigned int port)
{    
    if(rtde_receive != nullptr)
    {
        //disconnect();
        //rtde_control->reconnect();
        if(rtde_receive->isConnected())
        {
            rtde_receive->disconnect();           
        }
        else
        {
            rtde_receive->reconnect();
        }

        if(rtde_control->isConnected())
        {
            rtde_control->disconnect();
        }else
        {
            rtde_control->reconnect();
        }
    }
    else
    {
        qDebug()<<"urparser connect.....!";
        double dt=1.0/rtde_frequency;
        uint16_t flags= ur_rtde::RTDEControlInterface::FLAG_VERBOSE | ur_rtde::RTDEControlInterface::FLAG_UPLOAD_SCRIPT;
        //rtde_control=new ur_rtde::RTDEControlInterface(ip.toStdString());
        //rtde_control = new Control_interface(ip.toStdString(),rtde_frequency,flags,port,rt_control_priority);
        rtde_control = new ur_rtde::RTDEControlInterface(ip.toStdString(),rtde_frequency,flags,port,rt_control_priority);
        rtde_receive = new ur_rtde::RTDEReceiveInterface(ip.toStdString(),rtde_frequency,{},true,false,rt_receive_priority);
        rtde_io= new ur_rtde::RTDEIOInterface(ip.toStdString());

        //rtde_control->reconnect();
        //rtde=new ur_rtde::RTDE(ip.toStdString(),port);
        ur_rtde::RTDEUtility::setRealtimePriority(80);

//        std::vector<double> current = rtde_receive->getActualCurrent();
//        std::cout<<"current == ";
//        for(int i=0;i<current.size();++i)
//        {
//            std::cout<<current.at(i)<<" ";
//        }
//        std::cout<<std::endl;
//        std::vector<double> jt= rtde_receive->getActualQ();
//        std::cout<<"actual q == ";
//        for(int i=0;i<jt.size();++i)
//        {
//            std::cout<<jt.at(i)*180/M_PI<<" ";
//        }
//        std::cout<<std::endl;
//        std::vector<double> ps= rtde_receive->getActualTCPPose();
//        std::cout<<"actual tcp pos == ";
//        for(int i=0;i<ps.size();++i)
//        {
//            std::cout<<ps.at(i)<<" ";
//        }
//        std::cout<<std::endl;

    }
    //rtde_receive->reconnect();
    //rtde->connect();
    //rtde->getControllerVersion();

}

void URParser::disconnect()
{
    //rtde->disconnect();
    rtde_receive->disconnect();
}

bool URParser::isconnected()
{
    //return rtde->isConnected();
    return rtde_receive->isConnected();
}

void URParser::recive()
{
    //rtde->receive();
}

std::vector<double> URParser::getActualQ()
{
    if(!isconnected()) return{};

    std::vector<double> vec = rtde_receive->getActualQ();
    for(int i=0;i<vec.size();++i)
    {
        vec[i] *= (180/M_PI);
    }
    return vec;
}

std::vector<double> URParser::getActualTcpPos()
{
    if(!isconnected()) return{};
    std::vector<double> vec = rtde_receive->getActualTCPPose();
    if(vec.size()<6)
        return {};
    for(int i=0;i<3;++i)
    {
        vec[i]*=1000;
    }
    return vec;
}

std::vector<double> URParser::getActuralTcpForce()
{
    if(!isconnected()) return{};
    std::vector<double> vec = rtde_receive->getActualTCPForce();
    if(vec.size()<6)
        return {};
    return vec;
}

bool URParser::speedl(QString str)
{
    //qDebug()<<"speed L command:"<<str;
    std::vector<double> speed;
     QStringList list= str.split(",");
     if(list.size()<6)
         return false;
     for(int i=0;i<list.size();++i)
     {
         double t=list.at(i).toDouble();
         speed.push_back(t*M_PI/180);
     }
    //qDebug()<<"speed L command:"<<QVector<double>::fromStdVector(speed);
    if(!isrun)
    {
      if(rtde_control->speedL(speed,0.25,1))
          isrun=true;
    }
    return isrun;
}

bool URParser::speedl(std::vector<double> speed)
{
//    if(!isrun)
//    {
    std::vector<double> movesp;
    if(speed.size()==6)
    {
        for(int i=0;i<speed.size();++i)
        {
            movesp.push_back(speed.at(i)*speedfactor);
        }
    }

      if(rtde_control->speedL(movesp,0.25,0.1))
          isrun=true;
//    }
    return isrun;
}

bool URParser::stopspeedL()
{
    if(isrun )
    {
        if(rtde_control->speedStop(8))
            isrun=false;
    }
    return true;
}

void URParser::movej(QString str)
{
    std::vector<double> joint;
    QStringList list= str.split(",");
    if(list.size()<6)
        return;
    for(int i=0;i<list.size();++i)
    {
        double t=list.at(i).toDouble();
        joint.push_back(t*M_PI/180);
    }
    //qDebug()<<QVector<double>::fromStdVector(joint);
    if(rtde_control->isConnected())
    {
        if(joint.size()==6)
            rtde_control->moveJ(joint);
    }
}

int URParser::getIO_input(int ioID)
{
    if(!isconnected())
        return -1;
   if(rtde_receive->getDigitalInState(ioID))
       return 1;
   else
       return 0;
}

void URParser::setSpeedFactor(double fact)
{
    speedfactor=fact;
}
